#include <earth_rover_localization/correct_localization.h>

CorrectLocalization::CorrectLocalization(const ros::NodeHandle &node_handle, const ros::NodeHandle &private_node_handle):
m_nh(node_handle),
m_pnh(private_node_handle)
{
    // Load local params
    std::string gnss_ref_itopic, heading_itopic, ekf_global_itopic, status_itopic;
    std::string gnss_ref_otopic, heading_otopic, ekf_global_otopic;
    if (!m_nh.getParam("/correct_localization/gnss_ref_itopic", gnss_ref_itopic))
    {
        ROS_ERROR("[LOCALIZATION][CORRECT] Missing parameter: /correct_localization/gnss_ref_itopic");
    }
    if (!m_nh.getParam("/correct_localization/heading_itopic", heading_itopic))
    {
        ROS_ERROR("[LOCALIZATION][CORRECT] Missing parameter: /correct_localization/heading_itopic");
    }
    if (!m_nh.getParam("/correct_localization/ekf_global_itopic", ekf_global_itopic))
    {
        ROS_ERROR("[LOCALIZATION][CORRECT] Missing parameter: /correct_localization/ekf_global_itopic");
    }
    if (!m_nh.getParam("/correct_localization/status_itopic", status_itopic))
    {
        ROS_ERROR("[LOCALIZATION][CORRECT] Missing parameter: /correct_localization/status_itopic");
    }
    if (!m_nh.getParam("/correct_localization/gnss_ref_otopic", gnss_ref_otopic))
    {
        ROS_ERROR("[LOCALIZATION][CORRECT] Missing parameter: /correct_localization/gnss_ref_otopic");
    }
    if (!m_nh.getParam("/correct_localization/heading_otopic", heading_otopic))
    {
        ROS_ERROR("[LOCALIZATION][CORRECT] Missing parameter: /correct_localization/heading_otopic");
    }
    if (!m_nh.getParam("/correct_localization/ekf_global_otopic", ekf_global_otopic))
    {
        ROS_ERROR("[LOCALIZATION][CORRECT] Missing parameter: /correct_localization/ekf_global_otopic");
    }

    XmlRpc::XmlRpcValue xml_covar_rtk_fix, xml_covar_rtk_float, xml_covar_dgnss, xml_covar_sbas, xml_covar_spp, xml_covar_unknown;
    // Load global params
    if (!m_nh.getParam("/correct_localization/covar_rtk_fix", xml_covar_rtk_fix))
    {
        ROS_ERROR("[LOCALIZATION][CORRECT] Missing parameter: /correct_localization/covar_rtk_fix");
    }
    m_covar_rtk_fix = getArrayFromXmlValue(xml_covar_rtk_fix);
    if (!m_nh.getParam("/correct_localization/covar_rtk_float", xml_covar_rtk_float))
    {
        ROS_ERROR("[LOCALIZATION][CORRECT] Missing parameter: /correct_localization/covar_rtk_float");
    }
    m_covar_rtk_float = getArrayFromXmlValue(xml_covar_rtk_float);
    if (!m_nh.getParam("/correct_localization/covar_dgnss", xml_covar_dgnss))
    {
        ROS_ERROR("[LOCALIZATION][CORRECT] Missing parameter: /correct_localization/covar_dgnss");
    }
    m_covar_dgnss = getArrayFromXmlValue(xml_covar_dgnss);
    if (!m_nh.getParam("/correct_localization/covar_sbas", xml_covar_sbas))
    {
        ROS_ERROR("[LOCALIZATION][CORRECT] Missing parameter: /correct_localization/covar_sbas");
    }
    m_covar_sbas = getArrayFromXmlValue(xml_covar_sbas);
    if (!m_nh.getParam("/correct_localization/covar_spp", xml_covar_spp))
    {
        ROS_ERROR("[LOCALIZATION][CORRECT] Missing parameter: /correct_localization/covar_spp");
    }
    m_covar_spp = getArrayFromXmlValue(xml_covar_spp);
    if (!m_nh.getParam("/correct_localization/covar_unknown", xml_covar_unknown))
    {
        ROS_ERROR("[LOCALIZATION][CORRECT] Missing parameter: /correct_localization/covar_unknown");
    }
    m_covar_unknown = getArrayFromXmlValue(xml_covar_unknown);
    if (!m_nh.getParam("/correct_localization/gps_latency", m_gps_latency))
    {
        ROS_ERROR("[LOCALIZATION][CORRECT] Missing parameter: /correct_localization/gps_latency");
    }
    if (!m_nh.getParam("/correct_localization/ekf_latency", m_ekf_latency))
    {
        ROS_ERROR("[LOCALIZATION][CORRECT] Missing parameter: /correct_localization/ekf_latency");
    }

    // Subscribers
    m_gps_sub = m_nh.subscribe(gnss_ref_itopic, 1000, &CorrectLocalization::gpsCb, this);
    m_heading_sub = m_nh.subscribe(heading_itopic, 1000, &CorrectLocalization::headingCb, this);
    m_ekf_sub = m_nh.subscribe(ekf_global_itopic, 1000, &CorrectLocalization::ekfCb, this);
    m_status_sub = m_nh.subscribe(status_itopic, 1000, &CorrectLocalization::statusCb, this);

    // Publishers
    m_gps_pub = m_nh.advertise<sensor_msgs::NavSatFix>(gnss_ref_otopic, 1000);
    m_heading_pub = m_nh.advertise<sensor_msgs::Imu>(heading_otopic, 1000);
    m_ekf_pub = m_nh.advertise<nav_msgs::Odometry>(ekf_global_otopic, 1000);

    ROS_INFO("[LOCALIZATION][CORRECT] Node initialized");
}

std::vector<float> CorrectLocalization::getArrayFromXmlValue(const XmlRpc::XmlRpcValue& param_in)
{
    std::vector<float> param_out(param_in.size());
    for (int i{0}; i<param_out.size(); i++)
    {
        try
        {
            std::ostringstream ostr;
            param_in[i].write(ostr);
            std::istringstream istr(ostr.str());
            istr >> param_out[i];
        }
        catch(XmlRpc::XmlRpcException &e)
        {
            throw e;
        }
        catch(...)
        {
            throw;
        }
    }
    return param_out;
}

void CorrectLocalization::statusCb(const earth_rover_common_messages::LocalizationStatus& msg)
{
    m_localization_status = msg;
}

void CorrectLocalization::gpsCb(const sensor_msgs::NavSatFixConstPtr& gps_msg)
{
      if (m_gps_latency==0.0){
          m_gps_pub.publish(*gps_msg);
      }
      else{
          sensor_msgs::NavSatFix gps_latency_msg;
          gps_latency_msg = *gps_msg;
          ros::Duration gps_latency_2rosdur(m_gps_latency*0.001);
          gps_latency_msg.header.stamp = gps_msg->header.stamp - gps_latency_2rosdur;
          m_gps_pub.publish(gps_latency_msg);
      }
}

void CorrectLocalization::headingCb(const sensor_msgs::ImuConstPtr& heading_msg)
{
      if (m_gps_latency==0.0){
        m_heading_pub.publish(*heading_msg);
      }
      else{
         sensor_msgs::Imu heading_latency_msg;
         heading_latency_msg = *heading_msg;
         ros::Duration gps_latency_2rosdur(m_gps_latency*0.001);
         heading_latency_msg.header.stamp = heading_msg->header.stamp - gps_latency_2rosdur;
         m_heading_pub.publish(heading_latency_msg);
     }
}

void CorrectLocalization::ekfCb(const nav_msgs::OdometryConstPtr& ekf_msg)
{
      nav_msgs::Odometry ekf_corrected_msg;
      ekf_corrected_msg = *ekf_msg;
      ros::Duration ekf_latency_2rosdur(m_ekf_latency*0.001);
      ekf_corrected_msg.header.stamp = ekf_msg->header.stamp - ekf_latency_2rosdur;
      logicCovariance(ekf_corrected_msg, m_localization_status);
      m_ekf_pub.publish(ekf_corrected_msg);
}

void CorrectLocalization::logicCovariance(nav_msgs::Odometry& odom, const earth_rover_common_messages::LocalizationStatus& status)
{
    // POSITION
    if (status.gnss_reference == earth_rover_common_messages::LocalizationStatus::GNSS_REFERENCE_FIXED_RTK)
    {
        odom.pose.covariance[0] = m_covar_rtk_fix[0]; //position in x
        odom.pose.covariance[7] = m_covar_rtk_fix[1]; //position in y
    }
    else if (status.gnss_reference == earth_rover_common_messages::LocalizationStatus::GNSS_REFERENCE_FLOAT_RTK)
    {
        odom.pose.covariance[0] = m_covar_rtk_float[0]; //position in x
        odom.pose.covariance[7] = m_covar_rtk_float[1]; //position in y
    }
    else if (status.gnss_reference == earth_rover_common_messages::LocalizationStatus::GNSS_REFERENCE_DGNSS)
    {
        odom.pose.covariance[0] = m_covar_dgnss[0]; //position in x
        odom.pose.covariance[7] = m_covar_dgnss[1]; //position in y
    }
    else if (status.gnss_reference == earth_rover_common_messages::LocalizationStatus::GNSS_REFERENCE_SBAS)
    {
        odom.pose.covariance[0] = m_covar_sbas[0]; //position in x
        odom.pose.covariance[7] = m_covar_sbas[1]; //position in y
    }
    else if (status.gnss_reference == earth_rover_common_messages::LocalizationStatus::GNSS_REFERENCE_SPP)
    {
        odom.pose.covariance[0] = m_covar_spp[0]; //position in x
        odom.pose.covariance[7] = m_covar_spp[1]; //position in y
    }
    else
    {
        odom.pose.covariance[0] = m_covar_unknown[0]; //position in x
        odom.pose.covariance[7] = m_covar_unknown[1]; //position in y
    }

    // ORIENTATION
    if (status.gnss_attitude == earth_rover_common_messages::LocalizationStatus::GNSS_ATTITUDE_FIXED_RTK)
    {
        odom.pose.covariance[35] = m_covar_rtk_fix[5]; // yaw orientation
    }
    else if (status.gnss_attitude == earth_rover_common_messages::LocalizationStatus::GNSS_ATTITUDE_FLOAT_RTK)
    {
        odom.pose.covariance[35] = m_covar_rtk_float[5]; // yaw orientation
    }
    else if (status.gnss_attitude == earth_rover_common_messages::LocalizationStatus::GNSS_ATTITUDE_DGNSS)
    {
        odom.pose.covariance[35] = m_covar_dgnss[5]; // yaw orientation
    }
    else
    {
        odom.pose.covariance[35] = m_covar_unknown[5]; //position in x
    }
}
